/*
 * <nav_recip_rca/rca_robot.h>
 * nav_recip_rca
 *
 *
 * Copyright (c) 2008-2012, University of North Carolina at Chapel Hill
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of the University of North Carolina at Chapel Hill nor
 *     the names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * Please send all bug reports to <geom@cs.unc.edu>.
 *
 * The authors may be contacted via:
 *
 *     Dept. of Computer Science
 *     201 S. Columbia St.
 *     Frederick P. Brooks, Jr. Computer Science Bldg.
 *     Chapel Hill, N.C. 27599-3175
 *     United States of America
 *
 *     http://gamma.cs.unc.edu/ROS/
 */

#ifndef NAV_RECIP_RCA_RCA_ROBOT_H_
#define NAV_RECIP_RCA_RCA_ROBOT_H_

#include <ostream>
#include <vector>
#include <ros/macros.h>
#include <nav_recip_common/nav_recip_common.h>
#include <nav_recip_rca/line.h>

namespace nav_recip
{
class RCARobot;

/*!
 * \relates RCARobot
 *
 * A list of Robot pointers.
 */
typedef std::vector<RCARobot*> RCARobotList;

/*!
 * \relates RCARobot
 *
 * An iterator for an RCARobotList.
 */
typedef std::vector<RCARobot*>::iterator RCARobotIterator;

/*!
 * \relates RCARobot
 *
 * A \c const iterator for an RCARobotList.
 */
typedef std::vector<RCARobot*>::const_iterator RCARobotConstIterator;

/*!
 * \headerfile <nav_recip_rca/rca_robot.h>
 *
 * A robot that avoids collisions with its neighbors in 2D workspace by
 * computing lines that bound its collision-avoiding velocities.
 */
class ROS_HELPER_EXPORT RCARobot : public Robot
{
public:
  /*!
   * Create a new RCARobot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL RCARobot() : Robot(), time_horizon_(0.0f), lines_() { }

  /*!
   * Create a new RCARobot with given bounding radius, current position, current
   * linear velocity, preferred linear velocity, maximum linear speed, and time
   * horizon.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL RCARobot(float radius, const Vector2& position, const Vector2& velocity,
                                             const Vector2& preferred_velocity, float max_speed, float time_horizon) :
      Robot(radius, position, velocity, preferred_velocity, max_speed), time_horizon_(time_horizon), lines_() { }

  // Return the list of lines that bound collision-avoiding velocities of this
  // RCARobot.
  ROS_FORCE_INLINE ROS_HELPER_LOCAL const LineList& getLines() const { return lines_; }

  /*!
   * Return the time horizon of this RCARobot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL float getTimeHorizon() const { return time_horizon_; }

  /*!
   * Set the time horizon of this RCARobot.
   */
  ROS_FORCE_INLINE ROS_HELPER_LOCAL void setTimeHorizon(float time_horizon) { time_horizon_ = time_horizon; }

  /*!
   * Compute the new linear velocity of this RCARobot that avoids collisions
   * with its neighbors.
   */
  virtual bool computeNewVelocity(float time_step);

  float time_horizon_; // Time horizon.
  LineList lines_;     // Lines bounding collision-avoiding velocities.

  // Inherited from Robot.

  // Return the maximum linear speed of this RCARobot.
  // float getMaxSpeed() const;

  // Return the list of neighbors of this RCARobot.
  // const ConstRobotList& getNeighbors() const;

  // Return the new linear velocity of this RCARobot.
  // const Vector2& getNewVelocity() const;

  // Return the current position of this RCARobot.
  // const Vector2& getPosition() const;

  // Return the preferred linear velocity of this RCARobot.
  // const Vector2& getPreferredVelocity() const;

  // Return the bounding radius of this RCARobot.
  // float getRadius() const;

  // Return the current linear velocity of this RCARobot.
  // const Vector2& getVelocity() const;

  // Set the maximum linear speed of this RCARobot.
  // void setMaxSpeed(float max_speed);

  // Set the list of neighbors of this RCARobot.
  // void setNeighbors(const ConstRobotList& neighbors);

  // Set the current position of this RCARobot.
  // void setPosition(const Vector2& position);

  // Set the preferred linear velocity of this RCARobot.
  // void setPreferredVelocity(const Vector2& preferred_velocity);

  // Set the bounding radius of this RCARobot.
  // void setRadius(float radius);

  // Set the current linear velocity of this RCARobot.
  // void setVelocity(const Vector2& velocity);

  // float max_speed_;            // Maximum linear speed.
  // float radius_;               // Bounding radius.
  // Vector2 new_velocity_;       // New linear velocity set by computeNewVelocity().
  // Vector2 position_;           // Current position.
  // Vector2 preferred_velocity_; // Preferred linear velocity.
  // Vector2 velocity_;           // Current linear velocity used by computeNewVelocity().
  // ConstRobotList neighbors_;   // Neighbors with which to avoid collisions.

private:
  // Compute the line constraints bounding collision-avoiding velocities.
  ROS_HELPER_LOCAL void computeLines(float time_step);

  // Solve a linear program subject to line constraints and a circle constraint.
  ROS_HELPER_LOCAL bool solveLinearProgram();

  // Solve a 1D linear program on a given line subject to given line constraints
  // and a given circle constraint.
  ROS_HELPER_LOCAL bool solveLinearProgram1D(const LineList& lines, size_t line_no, const Vector2& preferred_velocity,
                                             bool optimize_direction);

  // Solve a 2D linear program subject to given line constraints and a given
  // circle constraint.
  ROS_HELPER_LOCAL size_t solveLinearProgram2D(const LineList& lines, const Vector2& preferred_velocity,
                                               bool optimize_direction);

  // Solve a 3D linear program subject to line constraints and a given circle
  // constraint.
  ROS_HELPER_LOCAL void solveLinearProgram3D(size_t line_no);
};

/*!
 * \relates RCARobot
 *
 * Insert a given RCARobot into a given output stream
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL std::ostream& operator<<(std::ostream& output_stream, const RCARobot& robot)
{
  output_stream << "[ RCARobot: radius = " << robot.getRadius() << ", position = " << robot.getPosition()
      << ", velocity = " << robot.getVelocity() << " ]";

  return output_stream;
}
}

#endif // NAV_RECIP_RCA_RCA_ROBOT_H_
